/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.nurdrobotics;

import edu.wpi.first.wpilibj.Joystick;

/**
 * 
 */
public class ShooterStick {

    // Need to check whether or not we're keepng pitch
    // Need to check if we're using a different device to control shooter speed

    // The following variables gives a name to the port that is connected
    // to the shooter joystick.
    // This way, it's easy to look at this section of code to make sure that
    // the shooter joystick is configured correctly in the DriverStation

    private static final double SHOOTER_SPEED_MIN = 0.0;
    private static final double SHOOTER_SPEED_MAX = 1.0;
    private static final int PORT_SHOOTER = 2;
    // The following variables define names for the axes on the shooter
    // joystick
    private static final int AXIS_PITCH = 2;
    private static final int AXIS_THUMB = 3;

    // The follwoing variables define names for all the buttons on the
    // shooter joystick; 2 no-op, 3 no-op
    private static final int BUTTON_SHOOT = 1;
    private static final int BUTTON_SHOOT_AUTO = 4;
    private static final int BUTTON_PITCH_STICK_ENABLE = 2;
    //private static final int BUTTON_AUTO_PITCH_UP = 6;
    //private static final int BUTTON_AUTO_PITCH_DOWN = 7;
    private static final int BUTTON_EJECT = 8;
    private static final int BUTTON_DISABLE_SHOOTER = 9;
    private static final int BUTTON_LIFTER_MANUAL = 10;
    //private static final int BUTTON_STAGER_MANUAL = 11;
    
    // declare a variable for holding the joystick
    private static Joystick m_joystick = null;
    // create boolean values to remember the last state of buttons
    // for rising edges
    private static boolean shootLastPressedRising = false;
    private static boolean reverseLastPressedRising = false;
    // create rising edge detection for shooter disabled button
    private static boolean disabled = false;
    private static boolean disabledButtonPressedLast = false;

    /*
     * This function initializes variables in this class. It should
     * be called exactly once prior to using the class.
     */
    public static boolean init() {
        // initialize the joystick
        m_joystick = new Joystick(PORT_SHOOTER);
        if (m_joystick == null) {
            return false;
        }
        return true;
    }

    /*
     * Returns true if the deploy wedge button is pressed, false otherwise
     */
    public static boolean shootPressed() {
        return (m_joystick.getRawButton(BUTTON_SHOOT));
    }

    //line 69
    /*
     * This routine returns true if the shoot button is pressed and wasn't
     * pressed the prior time this routine was called.
     */
    public static boolean shootRisingEdge() {
        // determine whether the button is currently pressed
        boolean pressed = shootPressed();

        // by default, set the return value to reflect this is not a rising edge
        boolean returnValue = false;

        // if the button is currently pressed, and it wasn't pressed last time
        // then set the return value to reflect this is a rising edge
        if (pressed && (shootLastPressedRising == false)) {
            returnValue = true;
        }

        // update the last pressed value to the currently pressed value
        shootLastPressedRising = pressed;

        // return the result of whether or not this was a rising edge
        return returnValue;
    }

    /*
     * Returns true if the deploy wedge button is pressed, false otherwise
     */
    public static boolean reversePressed() {
        return (m_joystick.getRawButton(BUTTON_EJECT));
    }

    //Creates four methods that return the state (T or F) of the
    //manual stager, lifter, and pitch buttons
    public static boolean manualStagerPressed() {
        //return (m_joystick.getRawButton(BUTTON_STAGER_MANUAL));
        return false;
    }

    public static boolean manualLifterPressed() {
        return (m_joystick.getRawButton(BUTTON_LIFTER_MANUAL));
    }

    public static boolean autoExtendPressed() {
        //return (m_joystick.getRawButton(BUTTON_AUTO_PITCH_UP));
        return false;
    }

    public static boolean autoRetractPressed() {
        //return (m_joystick.getRawButton(BUTTON_AUTO_PITCH_DOWN));
        return false;
    }

    /*
     * This routine returns true if the reverse button is pressed and wasn't
     * pressed the prior time this routine was called.
     */
    public static boolean reverseRisingEdge() {
        // determine whether the button is currently pressed
        boolean pressed = reversePressed();

        // by default, set the return value to reflect this is not a rising edge
        boolean returnValue = false;

        // if the button is currently pressed, and it wasn't pressed last time
        // then set the return value to reflect this is a rising edge
        if (pressed && (reverseLastPressedRising == false)) {
            returnValue = true;
        }

        // update the last pressed value to the currently pressed value
        reverseLastPressedRising = pressed;

        // return the result of whether or not this was a rising edge
        return returnValue;
    }

    /*
     * Returns true if the deploy wedge button is pressed, false otherwise
     */
    public static boolean fullAutoPressed() {
        return (m_joystick.getRawButton(BUTTON_SHOOT_AUTO));
    }

    public static boolean disableShooter() {
        boolean buttonPressed = m_joystick.getRawButton(BUTTON_DISABLE_SHOOTER);

        if (buttonPressed == true && buttonPressed != disabledButtonPressedLast) {
            disabled = !disabled;
        }

        disabledButtonPressedLast = buttonPressed;

        return disabled;
        //(m_joystick.getRawButton(BUTTON_DISABLE_SHOOTER));
    }

    public static double getMotorSpeed() {
        double position = -m_joystick.getRawAxis(ShooterStick.AXIS_THUMB);
        //scales from 0 to 1.0
        position = (position + 1) / 2;

        if (position > SHOOTER_SPEED_MAX) {
            position = SHOOTER_SPEED_MAX;
        } else if (position < SHOOTER_SPEED_MIN) {
            position = SHOOTER_SPEED_MIN;
        }

        return position;
    }

    public static double getPitchValue() {
        // get the move value from the joystick
        if (m_joystick.getRawButton(BUTTON_PITCH_STICK_ENABLE)) {
            return -m_joystick.getRawAxis(ShooterStick.AXIS_PITCH);
        } else {
            return 0;
        }
    }
}
